#include "navigator.h"
#include <stdio.h>
#include "node.h"
#include <vector>
#include <math.h>
#include "windows.h"

namespace nav
{
//<defines>
#define TURN_45_LOSTACCURACY   0.23f
#define MOVE_UOM_LOSTACCURACY  0.42f
#define uint unsigned int
#define UNEXPLORED 0
#define OBSTICLE  65533
#define END )

//</defines>

//Constructor of Navigator Class: needs Target Coordinates, start Coordinates, a startDirection , and the size of the Map
Navigator::Navigator(int targetX, int targetY, int startX, int startY,  nav::dirEncoded startDirection,  int mapSizeX, int mapSizeY):  Map( mapSizeX, mapSizeY)
{

    this->dir=startDirection;
    this->PositionX=startX;
    this->PositionY=startY;
    this->targetX=targetX;
    this->targetY=targetY;

}

//Destructor Removes the Navigator, should a new Target be aquired
Navigator::~Navigator()
{
    //Calls the destructor of the internal field -which in turn calls the Destructor of the Nodes
    Map.~field();
}

//Method compares the current Position with the target
bool Navigator::TargetReached()
{
    return (this->PositionX==this->targetX && this->targetY==this->PositionY);
}

//do a turn, returns the actual done turn as determinated by the mouse
float Navigator::doATurn(dirEncoded DirToTurn, bool* Success)
{
    //TODO METHOD NOT IMPLEMENTED
    //Here the Mouse Sensor strikes again- using the Vehicle pivot we can almost perfectly determinate the MovementVector of the Sensor
    //return is either zero on a perfect turn or represent the lost accuracyProTurnPart*NrOfTurns
    *Success=true;
    return 0;
}


//This is were the A* gets executed on the currently known Map, it returns the next Node the Robot should move towards
Node* Navigator::goForIt(int xStart, int yStart,int xGoal, int yGoal)
{
    Node* Start= this->Map.getNodeAt(xStart,yStart);
    Node* goal= this->Map.getNodeAt(xGoal,yGoal);

    std::vector<Node*> path;
    std::vector<Node*> closedSet;    // The set of nodes already evaluated.
    std::vector<Node*> openSet;   // The set of tentative nodes to be evaluated, initially containing the start node

    //a ring thing - you wouldnt understand
    Start->predecessorPtr=this->Map.getNodeAt(Start->posX,Start->posY);

    //Start Knoten gewichtet mit der Luftlinie zum Ziel
    Start->tweight=0 + heuristicDistance(Start->posX,xGoal,Start->posY,yGoal);
    Start->explored=true;
    Start->accumulatedPathCosts=0;

    openSet.push_back(Start);
    int currentLowestPathCosts=Start->weight;
    int counter=0;
    //solang das offene Knotenset nicht leer ist
    while(!openSet.empty() )
    {
        counter++;
        //Hol dir den kleinsten Knoten
        Node* current = this->findLowestInSet(openSet);
        current->infoPath=true;
        currentLowestPathCosts=current->tweight;
        path.push_back(current);

        //DelMe
            if (counter %100==0)
            this->Map.displayMap(this->PositionX,this->PositionY,this->targetX,this->targetY);;


        this->Map.setNodeAt(current->posX,current->posY,current);

         //Targetnode reached
         if (current->Equals(goal))
            {
               // (*path.back()).PlotNodeData();
               // return path[2];
                return this->tracePathBack(current,Start,closedSet);
            }

        // remove current from openset
        int position= this->findPositionInSet(openSet,current);
            if ( position != -1)
            {
        //    printf("OpenSet current removal"<<position END;
            openSet.erase(openSet.begin(),openSet.begin()+1);
            }

        // add current to closedset
        closedSet.push_back(current);

        //we got the current Neighbours
        std::vector<Node*> neighbor_nodes= this->Map.getNeighbours(current);

        //itterating over the found Neighbours
        for (uint i=0; i< neighbor_nodes.size(); i++)
        {
            //printf("Checking Neighbour at"<<neighbor_nodes[i].posX<< " / " <<neighbor_nodes[i].posY);
            //Neighbournode has been allready evaluated

            if (this->findPositionInSet(closedSet,neighbor_nodes[i])!=-1|| neighbor_nodes[i]->weight== OBSTICLE )
            {
                continue;
            }

            //setting the predecessor
            neighbor_nodes[i]->setPredecessor(this->Map.getNodeAt(current->posX,current->posY))   ;

            //consisting off         costs so far to get here
            neighbor_nodes[i]->accumulatedPathCosts=current->accumulatedPathCosts+ 1 ;
            neighbor_nodes[i]->tweight=current->accumulatedPathCosts+ 1 + heuristicDistance(neighbor_nodes[i]->posX,goal->posX,neighbor_nodes[i]->posY,goal->posY);

            //  printf("Node "<<i<<" accumulated tweight"<<  neighbor_nodes[i].tweight;

            //if the neighbour was not yet in the open set              or currentPathCosts are < his path costs
            if (this->findPositionInSet(openSet,neighbor_nodes[i])== -1 ||  neighbor_nodes[i]->tweight <=  currentLowestPathCosts )
            {
//                printf("Added Neighbournode to OpenSet" END;
                currentLowestPathCosts=neighbor_nodes[i]->tweight;
                openSet.push_back(neighbor_nodes[i]);
            }

            //we set the changes down to the Map
            this->Map.setNodeAt(neighbor_nodes[i]->posX,neighbor_nodes[i]->posY, neighbor_nodes[i]);
        }



    }
    return Start;
}
// Method Finds the lowest weighted element in that set
//TODO make method more generic by taking a Operator
Node* Navigator::findLowestInSet(std::vector<Node*> Set)
{
    int temp_O_Rary_Weight= OBSTICLE;
    int popDatElement=0;

    for (unsigned int i=0; i<Set.size(); i++)
    {
        if (Set[i]->tweight < temp_O_Rary_Weight)
        {
            popDatElement=i;
            temp_O_Rary_Weight=Set[i]->tweight;
        }
    }
    return Set[popDatElement];
}

//Method calcs the traditional no-obsticle Distance between too nodes
int Navigator::heuristicDistance(int AposX, int BposX, int AposY, int BposY)
{
    int valA= (AposX-BposX);
    int valB= (AposY-BposY);
 //  printf("heuristicDistance is" <<(int)(sqrt((double)(valA*valA + valB*valB))+0.5f));
    return (sqrt((valA*valA + valB*valB)));
}

//Method moves the robot to the specified targetNode, which is always a NeighbourNode of the curretn One
float Navigator::makeAMove(Node* TargetNode, dirEncoded dir, bool *MoveSuccessPtr)
{
    //TODO METHOD NOT IMPLEMENTED
    printf("Make a Move");
 //   //Sleep(5000);
    //execute the movement and handle the MoveDistortion
    //TODO DelME //arbitrary testvalues
    this->dist_UPLOW +=1;
    this->dist_R_L +=1;
    this->dist_UPL_LOWR +=1;
    this->dist_UPR_LOWL +=1;
    //EndDelMe

    this->dir=dir;
    this->PositionX=TargetNode->posX;
    this->PositionY=TargetNode->posY;
    *MoveSuccessPtr=true;

    //TODO INSERT MOUSE SENSOR CODE HERE
    //The Mouse Sensor verifys the Movement done and returns a float which contains either zero (which means success)
    //or the lost accuracy in the testcase the perfect movement happens
    return 0.0f;
};

//Helper Method for GoForIt - rectracing the path, by following the Pointers of the Previeous Nodes
Node* Navigator::tracePathBack(Node* current,Node* Start,std::vector<Node*> closedSet)
{
    Node * cNodePtr= current;
    while  (!(cNodePtr->getPredecessor() == Start))
    {
       // //Sleep(500);
       //printf("Going back one Step");
        if (cNodePtr->getPredecessor()== current)
        {
       printf("End reached");
        //Sleep(1000);
        return cNodePtr;
        }
        cNodePtr=   cNodePtr->getPredecessor();

    }

    return cNodePtr;
};

//Helper Method searches for a Nod in a handed down Set
int Navigator::findPositionInSet(std::vector<Node*> Set, Node* FindMe)
{
    for (uint i =0      ; i<Set.size(); i++)
    {

        if (Set[i]->Equals(FindMe))
        {
          // printf("Found Position in Set");
            return i;
        }

    }

    return -1;
}
        //Method blends a Turn into the Map by orthogonal blurring it
        void Navigator::blendTurnIntoMap(dirEncoded startDir,dirEncoded targetDir, float LostAccuracy,bool OperationSuccess)
        {

        }

        void Navigator::blendMoveIntoMap(dirEncoded currDir,float LostAccuracy,bool OperationSuccess)
        {

        }


//Method does a sensor Sweep towards the Direction the robot is currently facing
//TODO How to handle multiple direction input
//TODO How to handle SensorValues who do contradicton one another?
//Pessimistic: The Negative autcome (Obsticle) always wins
//Optimist: Always assume there is
int Navigator::doSensorSweepInDir()
{
    dirEncoded currentDir= this->dir;

    //TODO US-Sensor functions


    //TODO METHOD NOT IMPLEMENTED
    return 0;
};

}
